#pragma config(Sensor, dgtl1,  lateralSensor,  sensorSONAR_cm)
#pragma config(Sensor, dgtl3,  wheelEncoder,   sensorQuadEncoder)
#pragma config(Sensor, dgtl5,  leftSensor,     sensorTouch)
#pragma config(Sensor, dgtl6,  rightSensor,    sensorTouch)
#pragma config(Sensor, dgtl7,  clawSensor,     sensorTouch)
#pragma config(Sensor, dgtl8,  frontalSensor,  sensorSONAR_cm)
#pragma config(Motor,  port2,           frontMotor,    tmotorServoContinuousRotation, openLoop, reversed)
#pragma config(Motor,  port3,           clawMotor,     tmotorServoContinuousRotation, openLoop, reversed)
#pragma config(Motor,  port4,           upMotor,       tmotorServoStandard, openLoop)
#pragma config(Motor,  port5,           rotationMotor, tmotorServoStandard, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

const int DimGrid = 24;
const int lateralMovement=6;
const int NZZ = DimGrid/(lateralMovement*2);
const int speed = 60;
const int extend = -127;
const int timeMoveUp = 1200;
const int timeRotate = 30;
const int time = 500;
const int time2 = 3000;
const int time3 = 500;
const int timeBackward = 500;
const int timeClaw = 200;
const float length = 7/3.25;
const float width = 5/3.25;
const float C = 1.5;
const int wall = 10;
const int bottleLength = 4;
const int clawSpeed = 50;
const byte ERR = 1;
const int degree = 92;
const int out = 3;


float X,Y,XO,YO;//0:front  1:right  2:down  3:left
byte ORI;
bool turn,move,END,ENDR;
byte map[DimGrid][DimGrid];

task position(){
  while(!END){
    float ang=0;
    SensorValue[wheelEncoder]=0;
    while(move&&!turn){
      ang=SensorValue[wheelEncoder];
      if(ang>100){ang=360-ang;}
      SensorValue[wheelEncoder]=0;
      switch(ORI){
        case 0:Y+=ang/90;break;
        case 1:X+=ang/90;break;
        case 2:Y-=ang/90;break;
        case 3:X-=ang/90;break;
      }
      wait1Msec(time);
    }
    wait1Msec(time);
  }
}

float sonar(char i){
  float s=(i=='f')?SensorValue(frontalSensor):SensorValue(lateralSensor);
  if(s>1&&s<20)return s;
  else return 100;
}
void add(int x,int y){
  if(x<DimGrid&&x>0&&y>0&&y<DimGrid&&map[x][y]<127){map[x][y]++;}
}

task relevation(){
  for(int i=0;i<DimGrid;i++)
    for(int j=0;j<DimGrid;j++)
      map[i][j]=0;
  while(!ENDR){
    if(sonar('f')<100){
      switch(ORI){
        case 0:add(X,Y+length); break;
        case 1:add(X+length,Y); break;
        case 2:add(X,Y-length); break;
        case 3:add(X-length,Y); break;
      }
    }
    if(sonar('l')<100){
      switch(ORI){
        case 3:add(X,Y+width); break;
        case 0:add(X+width,Y); break;
        case 1:add(X,Y-width); break;
        case 2:add(X-width,Y); break;
      }
    }
  }
}

void turnLR2(int dir){//1:right  -1:left
  if(dir<0){
    motor[rotationMotor]=-degree;
    wait1Msec(timeMoveUp*1.5);
    motor[upMotor]=extend;
    wait1Msec(timeMoveUp);
    for(int i=-degree;i<degree;i++){
      wait1Msec(timeRotate);
      motor[rotationMotor]=i;
    }
  }else{
    motor[rotationMotor]=degree;
    wait1Msec(timeMoveUp*1.5);
    motor[upMotor]=extend;
    wait1Msec(timeMoveUp);
    for(int i=-degree;i<degree;i++){
      wait1Msec(timeRotate);
      motor[rotationMotor]=-i;
    }
  }
  motor[upMotor]=-extend;
  wait1Msec(timeMoveUp);
  motor[rotationMotor]=0;
  wait1Msec(timeMoveUp*1.5);
}
void turnLR(int dir){
  turn=true;
 // turnLR2(dir);
  turnLR2(dir);
  turn=false;
  ORI+=dir;
  if(ORI>3){ORI-=4;}
  if(ORI<0){ORI+=4;}
}
void moveStraight2(int cel){
  float x=X;
  float y=Y;
  switch(ORI){
    case 0:y=Y+cel; break;
    case 1:x=X+cel; break;
    case 2:y=Y-cel; break;
    case 3:x=X-cel; break;
  }
  while((abs(x-X)>ERR||abs(y-Y)>ERR)){
    move=true;
    motor[frontMotor]=speed;
  }
  move=false;
  motor[frontMotor]=0;
}
bool moveStraight(int cel,bool keep){
  float x=X;
  float y=Y;
  bool found=false;
  bool b;
  switch(ORI){
    case 0:{y=Y+cel; b=Y>y; break;}
    case 1:{x=X+cel; b=X>x; break;}
    case 2:{y=Y-cel; b=Y<y; break;}
    case 3:{x=X-cel; b=X<x; break;}
  }
  while(!b&&!found){
    move=true;
    motor[frontMotor]=speed;
    if(sonar('f')<wall){
      move = false;
      motor[frontMotor]=0;
      int obj=0;
      for(int i=0;i<100;i++){if(sonar('f')<wall)obj++;}
      if(obj>=90&&!keep){
        motor[frontMotor]=-speed;
        wait1Msec(timeBackward);
        motor[frontMotor]=0;
        turnLR(-1);
        moveStraight2(bottleLength);
        turnLR(1);
        moveStraight2(bottleLength*2.5);
        turnLR(1);
        moveStraight2(bottleLength);
        turnLR(-1);
      }else if(obj>=90){
        found=true;
      }
    }
    switch(ORI){
      case 0:{b=Y>y; break;}
      case 1:{b=X>x; break;}
      case 2:{b=Y<y; break;}
      case 3:{b=X<x; break;}
    }
  }
  motor[frontMotor]=0;
  move=false;
  return found;
}
void think(){
  float middle=0;
  int i=0;
  for (int x=0;x<DimGrid;x++)
    for (int y=0;y<DimGrid;y++)
       if (map[x][y]!=0){
         i++;
         middle+=map[x][y];
       }
  middle/=i;
  i=0;
  XO = 0;
  YO = 0;
  for (int x=0;x<DimGrid;x++)
    for (int y=0;y<DimGrid;y++){
      map[x][y]=(map[x][y]>=middle*C)?1:0;
      if(map[x][y]==1){
        i++;
        XO+=x;
        YO+=y;
      }
    }
  XO/=i;
  YO/=i;
}
void pickUp(){
  motor[frontMotor]=-speed;
  wait1Msec(timeBackward);
  motor[frontMotor]=0;
  turnLR(1);
  turnLR(1);
  motor[clawMotor]=-clawSpeed;
  wait1Msec(time3);
  while(!SensorValue(clawSensor)){
    motor[frontMotor]=-speed;
  }
  motor[frontMotor]=0;
  motor[clawMotor]=clawSpeed;
  wait1Msec(time3);
  motor[clawMotor]=0;
}
void putDown(){
  motor[frontMotor]=speed;
  wait1Msec(time3);
  motor[frontMotor]=0;
  motor[clawMotor]=-clawSpeed;
  wait1Msec(timeClaw);
  motor[clawMotor]=0;
}

task main(){
  X=0;
  Y=0;
  ORI=0;
  turn = false;
  END=false;
  motor[upMotor]=-extend;
  wait1Msec(1000);
  StartTask(position);
  StartTask(relevation);
  int i=0;
  while(i<NZZ){
    moveStraight(DimGrid+out+((i!=0)?out:0),false);
    turnLR(1);
    moveStraight(lateralMovement,false);
    turnLR(1);
    moveStraight(DimGrid+out*2,false);
    if(i<NZZ-1){
      turnLR(-1);
      moveStraight(lateralMovement,false);
      turnLR(-1);
    }
    i++;
  }
  ENDR=false;
  turnLR(1);
  moveStraight(DimGrid-lateralMovement,false);
  turnLR(1);
  think();
  wait1Msec(time2);
  bool found=false;
  i=0;
  while(i<NZZ&&!found){
    found = moveStraight(DimGrid+out*2,true);
    if (!found){
      turnLR(1);
      moveStraight(lateralMovement,false);
      turnLR(1);
      found = moveStraight(DimGrid+out*2,true);
      if(!found){
        turnLR(-1);
        moveStraight(lateralMovement,false);
        turnLR(-1);
        i++;
      }
    }
  }
  if (found){
    pickUp();
    if(X>XO){
      switch(ORI){
        case 1:turnLR(1);turnLR(1); break;
        case 2:turnLR(1); break;
        case 0:turnLR(-1); break;
      }
      moveStraight(X-XO,false);
    }else if(X<XO){
      switch(ORI){
        case 3:turnLR(1);turnLR(1); break;
        case 2:turnLR(-1); break;
        case 0:turnLR(1); break;
      }
      moveStraight(XO-X,false);
    }
    if(Y>YO){
      switch(ORI){
        case 0:turnLR(1);turnLR(1); break;
        case 1:turnLR(1); break;
        case 3:turnLR(-1); break;
      }
      moveStraight(Y-YO,false);
    }else if(Y<YO){
      switch(ORI){
        case 2:turnLR(1);turnLR(1); break;
        case 1:turnLR(-1); break;
        case 3:turnLR(1); break;
      }
      moveStraight(YO-Y,false);
    }
    putDown();
  }
  ENDR=true;
  END=true;
}
